#include "vio/front_end/frame.h"
#include "vio/vio_common.h"

namespace vio {

int Frame::frame_counter_ = 0;

Frame::Frame(Camera::Ptr cam, const cv::Mat& img, const double& ts)
    : id_(frame_counter_++),
      timestamp_(ts),
      cam_ptr_(cam),
      key_pts_(5),
      is_keyframe_(false) {
  InitFrame(img);  // image pyramid
}

void Frame::InitFrame(const cv::Mat& img) {
  // check image
  if (img.empty() || img.type() != CV_8UC1) {
    LOG(FATAL) << "image is not grayscale";
  }
  // set keypoints to null
  std::for_each(key_pts_.begin(),
                key_pts_.end(),
                [&](std::shared_ptr<Feature> ftr) { ftr = nullptr; });
  // build image pyramid
  frame_utils::CreateImgPyramid(img, 3, img_pyr_);
}

void Frame::SetKeyFrame() {
  is_keyframe_ = true;
  SetKeyPoints();
}

void Frame::AddFeature(std::shared_ptr<Feature> ftr) {
  features_.push_back(ftr);
}

void Frame::SetKeyPoints() {
  for (size_t i = 0; i < 5; i++) {
    if (key_pts_[i] != nullptr) {
      if (key_pts_[i]->point_ptr.expired()) {
        key_pts_[i] = nullptr;
      }
    }
  }
  std::for_each(
      features_.begin(), features_.end(), [&](std::shared_ptr<Feature> ftr) {
        if (!ftr->point_ptr.expired()) {
          CheckKeyPoints(ftr);
        }
      });
}

// TODO(llhuang) different from origin svo
void Frame::CheckKeyPoints(std::shared_ptr<Feature> ftr) {
  const int cu = cam_ptr_->width() / 2;
  const int cv = cam_ptr_->height() / 2;

  // select the feature closest to center[cu, cv]
  if (key_pts_[0] == nullptr) {
    key_pts_[0] = ftr;
  } else if (std::max(std::fabs(ftr->px[0] - cu), std::fabs(ftr->px[1] - cv)) <
             std::max(std::fabs(key_pts_[0]->px[0] - cu),
                      std::fabs(key_pts_[0]->px[1] - cv))) {
    key_pts_[0] = ftr;
  }
  // right-up side, select the feature far away from center
  if (ftr->px[0] >= cu && ftr->px[1] >= cv) {
    if (key_pts_[1] == nullptr) {
      key_pts_[1] = ftr;
    } else if ((ftr->px[0] - cu) * (ftr->px[1] - cv) >
               (key_pts_[1]->px[0] - cu) * (key_pts_[1]->px[1] - cv)) {
      key_pts_[1] = ftr;
    }
  }
  // right-down side, select the feature far away from center
  if (ftr->px[0] >= cu && ftr->px[1] < cv) {
    if (key_pts_[2] == nullptr) {
      key_pts_[2] = ftr;
    } else if ((ftr->px[0] - cu) * (cv - ftr->px[1]) >
               (key_pts_[2]->px[0] - cu) * (cv - key_pts_[2]->px[1])) {
      key_pts_[2] = ftr;
    }
  }
  // left-down side
  if (ftr->px[0] < cu && ftr->px[1] < cv) {
    if (key_pts_[3] == nullptr) {
      key_pts_[3] = ftr;
    } else if ((cu - ftr->px[0]) * (cv - ftr->px[1]) >
               (cu - key_pts_[3]->px[0]) * (cv - key_pts_[3]->px[1])) {
      key_pts_[3] = ftr;
    }
  }
  // left-up side
  if (ftr->px[0] < cu && ftr->px[1] >= cv) {
    if (key_pts_[4] == nullptr) {
      key_pts_[4] = ftr;
    } else if ((cu - ftr->px[0]) * (ftr->px[1] - cv) <
               (cu - key_pts_[4]->px[0]) * (key_pts_[4]->px[1] - cv)) {
      key_pts_[4] = ftr;
    }
  }
}

void Frame::RemoveKeyPoint(std::shared_ptr<Feature>& ftr) {
  bool found = false;
  std::for_each(
      key_pts_.begin(), key_pts_.end(), [&](std::shared_ptr<Feature> i) {
        if (i == ftr) {
          i = nullptr;
          found = true;
        }
      });
  if (found) {
    SetKeyPoints();
  }
}

bool Frame::IsVisible(const Eigen::Vector3d& xyz_w) const {
  Eigen::Vector3d xyz_cam = T_f_w_ * xyz_w;
  if (xyz_cam[2] < 0.0) {
    // point is behind the camera
    return false;
  }
  Eigen::Vector2d px = u2p(xyz_cam);
  if (px[0] >= 0.0 && px[1] >= 0.0 && px[0] < cam_ptr_->width() &&
      px[1] < cam_ptr_->height()) {
    return true;
  }
  return false;
}

// utility functions for the Frame class
namespace frame_utils {
void CreateImgPyramid(const cv::Mat& img_level_0,
                      const int& n_levels,
                      ImgPyr& pyr) {
  pyr.resize(n_levels);
  pyr[0] = img_level_0;
  // SSE or NEON speedup
  for (int i = 1; i < n_levels; i++) {
    cv::resize(
        pyr[i - 1], pyr[i], cv::Size(pyr[i - 1].cols / 2, pyr[i - 1].rows / 2));
  }
}

bool GetScenseDepth(const Frame& frame, double& depth_mean, double& depth_min) {
  std::vector<double> depth_vec;
  depth_vec.reserve(frame.features_.size());
  depth_min = std::numeric_limits<double>::max();
  for (auto it = frame.features_.begin(); it != frame.features_.end(); ++it) {
    if (auto p_ptr = (*it)->point_ptr.lock()) {
      const double z = frame.w2c(p_ptr->pos_).z();
      depth_vec.push_back(z);
      depth_min = std::fmin(z, depth_min);
    }
  }
  if (depth_vec.empty()) {
    LOG(INFO) << "cannot set scene depth. Frame has no point-observations!";
    return false;
  }
  depth_mean = math_utils::GetMedian(depth_vec);
  return true;
}
}  // namespace frame_utils
}  // namespace vio
